/**
 * @file mode_monitor.cpp
 * @brief
 * @author haibo.yang
 * @version 1.0
 * @date 2023-08-15
 *
 * @copyright Copyright (c) 2023  lgim
 *
 */
#include "state_monitor/mode_monitor.h"
#include "state_monitor/event_manager.h"
#include <agv_msgs/msg/periph_motor_info.hpp>
#include <agv_msgs/msg/nav_type.hpp>
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <yaml-cpp/yaml.h>
#include "rclcpp/timer.hpp"
#include <shared_parameters/shared_params_interface.hpp>

namespace monitor
{
    
#define KEY_NAV_TYPE "agv_navigation_type"
#define AGV_MODE_FILE "/params/AGVinit.xml"
#define KEY_FLOOR_CUR "current_floor"
#define KEY_FLOOR_NUM "floor_num"

ModeMonitor::~ModeMonitor()
{   
    if (mode_pub_thread_.joinable())
        mode_pub_thread_.join();
}

void ModeMonitor::init()
{
    std::string path(getenv("HOME"));
    file_name_ = path + AGV_MODE_FILE;

    estop_last_state_ = monitor::Monitor::GetVehicleSaftyState();

    float delay_time = 1.0f;
    shielded_peripherals_status_ = Monitor::GetNode()->get_parameter_or("mode_monitor/shielded_peripherals", false);
    delay_time = Monitor::GetNode()->get_parameter_or("mode_monitor/delay", 600.0f);

    if (delay_time < 0) delay_time = 0.0f;
    else if (delay_time > 600.0f) delay_time = 600.0f;

    auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(delay_time));

    start_delay_time_ = rclcpp::Duration(ns);

    Monitor::GetNode()->get_parameter_or("mode_monitor/key_press_delay", delay_time, 2.0f);

    if (delay_time < 0) delay_time = 1.0f;
    else if (delay_time > 5.0f) delay_time = 5.0f;

    for (int i=0;i<MODEKEYCOUNT;i++) {
        mode_key_input_[i].name = keyname[i];
        mode_key_input_[i].handler_ = [this](const std::string& name, const rclcpp::Time time, const bool state) {
            on_button_pressed(name, time, state);
        };
        mode_key_input_[i].long_press_threshold = delay_time;
    }

    MONITOR_INFO("[ModeMonitor] shielded_peripherals_status_ :" << shielded_peripherals_status_ 
        << " delay:" << start_delay_time_.seconds() << " key_press_delay:" << delay_time);

    periph_status_sub_ = Monitor::GetNode()->create_subscription<agv_msgs::msg::PeriphStatus>(
        "/periphStatus", 
        rclcpp::QoS(2),
        std::bind(&ModeMonitor::PeriphStatusCallBack, this, std::placeholders::_1)
    );

    pose_sub_ = Monitor::GetNode()->create_subscription<agv_msgs::msg::PoseWithConfidence>(
        "/pose",
        rclcpp::QoS(1),
        std::bind(&ModeMonitor::PoseCallBack, this, std::placeholders::_1)
    );

    nav_type_pub_ = Monitor::GetNode()->create_publisher<agv_msgs::msg::NavType>("/navigation_type", 2);

    agv_mode_pub_ = Monitor::GetNode()->create_publisher<agv_msgs::msg::AGVMode>("/agvmode", 1);

    realtime_agv_mode_pub_ = 
        std::make_shared<realtime_tools::RealtimePublisher<agv_msgs::msg::AGVMode>>(agv_mode_pub_);

    operation_mode_ = Monitor::GetNode()->create_service<agv_srvs::srv::SetOperationMode>(
        "/operation_mode",
        std::bind(&ModeMonitor::OperationModeCallback, this, std::placeholders::_1, std::placeholders::_2));

    navigate_type_ = Monitor::GetNode()->create_service<agv_srvs::srv::ChangeNavType>(
        "/change_nav_type",
        std::bind(&ModeMonitor::ChangeNavTypeCallback, this, std::placeholders::_1, std::placeholders::_2));

    cur_floor_server_ = Monitor::GetNode()->create_service<agv_srvs::srv::ChangeCurFloor>(
        "/change_cur_floor",
        std::bind(&ModeMonitor::ChangeCurFloorCallback, this, std::placeholders::_1, std::placeholders::_2));

    chassis_controller_client_ = Monitor::GetNode()->create_client<agv_srvs::srv::ControllerEnable>("/controller_enable");

    mode_save_timer_ = Monitor::GetNode()->create_wall_timer(
        std::chrono::seconds(1), [this]() { this->SaveAgvMode(); });

    mode_pub_thread_ = boost::thread(boost::bind(&ModeMonitor::ModePublishThread, this));

    InitAgvMode();
}

void ModeMonitor::RunOnce(const rclcpp::Time current_time)
{
    UpdateStatus(current_time);
}

void ModeMonitor::UpdateStatus(const rclcpp::Time time)
{
    SwitchMode(time);
}

void ModeMonitor::writeInitFile(const int type, const Mode mode, const std::string floor)
{
    std::ofstream outputFile;
    outputFile.open(file_name_, std::ios::out);

    if (outputFile.is_open()) {
        MONITOR_INFO("[ModeMonitor] write agv mode: " << mode << " agv naviagte type:" << type);
        YAML::Node yamlConfig;
        yamlConfig["agv_navigation_type"] = type;
        yamlConfig["agv_mode"] = (int)mode;
        yamlConfig["current_floor"] = floor;

        // 将YAML::Node对象序列化为字符串并写入文件
        outputFile << yamlConfig;
        outputFile.flush();
        outputFile.close(); // 关闭文件
    } else {
        MONITOR_ERROR("[ModeMonitor] Failed to open file for writing");
    }
}

void ModeMonitor::SaveAgvMode()
{
    if (!initialize_agv_mode_)
        return;

    cur_nav_type_ = getNavType();
    auto floor = getFloorParam();
    auto cur_floor = std::get<0>(floor);
    if (last_mode_ != cur_mode_ || last_nav_type_ != cur_nav_type_ || floor_.last_floor != cur_floor) {
       writeInitFile(cur_nav_type_, cur_mode_, cur_floor);
    }

    last_mode_ = cur_mode_;
    last_nav_type_ = cur_nav_type_;
    floor_.last_floor = cur_floor;
}

bool ModeMonitor::InitAgvMode()
{
    // 检查文件是否存在
    std::ifstream ifs(file_name_);
    if (!ifs.is_open()) {
        MONITOR_ERROR("[ModeMonitor] Init file:" << file_name_ << " does not exist, create it immediately!");
        // 如果文件不存在，则创建文件
        std::ofstream outfile(file_name_);
        outfile.close();

        init_nav_type_ = 0;
        init_mode_ = Mode_Manul;
        last_nav_type_ = init_nav_type_;
        cur_nav_type_ = init_nav_type_;
        writeInitFile(cur_nav_type_, cur_mode_, "");
        return false;
    }

    std::string content((std::istreambuf_iterator<char>(ifs)), std::istreambuf_iterator<char>());
    ifs.close();

    MONITOR_INFO("[ModeMonitor] load init pose xml file " << file_name_ << " content:" << content);
    try {
        
        YAML::Node yamlConfig = YAML::Load(content);
        if (!yamlConfig) {
            MONITOR_ERROR("[ModeMonitor] Read %s Faild. data is not yaml format" << file_name_);
            return false;
        }

        // add curFloor tag
        if (yamlConfig["current_floor"]) {
            auto init_floor = yamlConfig["current_floor"].as<std::string>();
            setCurFloor(init_floor);
            MONITOR_INFO("[ModeMonitor] init_floor=" << init_floor);
        } else {
            MONITOR_INFO("[ModeMonitor] init_floor can not find");
        }

        if (yamlConfig["agv_navigation_type"] && yamlConfig["agv_mode"]) {
            init_nav_type_ = yamlConfig["agv_navigation_type"].as<int>();
            init_mode_ = (enum Mode)yamlConfig["agv_mode"].as<int>();

            last_nav_type_ = init_nav_type_;
            cur_nav_type_ = init_nav_type_;
            MONITOR_INFO("[ModeMonitor] load init pose xml is ok! agv_navigation_type=" << 
                init_nav_type_ << ", agv_mode=" << init_mode_);

            setNavType(init_nav_type_);
        } else {
            MONITOR_ERROR("[ModeMonitor] file:" << file_name_ << " data is not exist");
            init_nav_type_ = 0;
            init_mode_ = Mode_Manul;
            last_nav_type_ = init_nav_type_;
            cur_nav_type_ = init_nav_type_;
            writeInitFile(cur_nav_type_, cur_mode_, "");
            return false;
        }
    } catch (const YAML::Exception &e) {
        MONITOR_ERROR("[ModeMonitor] load File format error " << e.what());
        return false;
    }
    return true;
}

int ModeMonitor::getNavType()
{
    return SharedParamsInterface::getParam<int>("AGVParam", "agv_navigation_type", 0);
}

bool ModeMonitor::setNavType(int nNavType)
{
    return SharedParamsInterface::setParam("AGVParam", "agv_navigation_type", nNavType);
}

bool ModeMonitor::setCurFloor(std::string floorName)
{   
    return SharedParamsInterface::setParam("AGVParam", "current_floor", floorName);
}

std::tuple<std::string, int> ModeMonitor::getFloorParam()
{
    std::string current = SharedParamsInterface::getParam<std::string>("AGVParam", "current_floor", "1_Floor");
    int num = SharedParamsInterface::getParam<int>("AGVParam", "floor_num", 1);

    return std::make_tuple(current, num);
}

void ModeMonitor::ModePublishThread()
{
    int interval = 20;
    MONITOR_INFO("[ModeMonitor] thread id:" << boost::this_thread::get_id() << " spid:" << syscall(SYS_gettid));

    while (rclcpp::ok()) {
        AgvModePublish();
        // Sleep for 10 milliseconds
        boost::this_thread::sleep_for(boost::chrono::milliseconds(interval));
    }
}

bool ModeMonitor::ChangeNavTypeCallback(const std::shared_ptr<agv_srvs::srv::ChangeNavType::Request> req, 
    std::shared_ptr<agv_srvs::srv::ChangeNavType::Response> res)
{
    bool ret = false;
    int type = req->type;

    MONITOR_DEBUG("[ModeMonitor] NavType Service call back:" << type);
    if (cur_mode_ != Mode_Manul) {
        res->success = false;
        res->message = std::string("failed; oprationMode is not manual; mode=") + std::to_string(cur_mode_) + ",last nav type:" + std::to_string(getNavType());
        return true;
    }

    std::string last_type = std::to_string(getNavType());
    ret = setNavType(type);
    cur_nav_type_ = getNavType();

    std::string msg = "ok; last type:" + last_type + ", cur type:" + std::to_string(cur_nav_type_);
    res->success = ret;
    res->message = msg;

    MONITOR_INFO("[ModeMonitor] Change Navgate type:" << ret << ", last_type:" << last_type << ", cur_type:" << cur_nav_type_);
    return true;
}

bool ModeMonitor::ChangeCurFloorCallback(const std::shared_ptr<agv_srvs::srv::ChangeCurFloor::Request> req, 
    std::shared_ptr<agv_srvs::srv::ChangeCurFloor::Response> res)
{
    auto tag = req->floor_name;
    auto p = getFloorParam();
    auto last = std::get<0>(p);
    MONITOR_DEBUG("[ModeMonitor] curFloor Service call back:" << tag);
    if (cur_mode_ != Mode_Manul) {
        res->success = false;
        res->message = std::string("failed; change floor  is not manual; mode=") + std::to_string(cur_mode_) + (",last floor:" + last);
        return true;
    }

    setCurFloor(tag);
    std::string msg = "ok; last floor:" + last + ", cur floor:" + tag;
    res->success = true;
    res->message = msg;
    MONITOR_INFO("[ModeMonitor] Change floor:" << ", last_floor:" << last << ", cur_floor:" << tag);
    return true;
}

void ModeMonitor::PeriphStatusCallBack(const agv_msgs::msg::PeriphStatus::ConstSharedPtr &msg)
{
    bool error_status = false;
    // for (auto iter : msg->periphMotor)
    // {
    //     MONITOR_DEBUG("[ModeMonitor] PeriphStatusCallBack :" << iter);
    //     if (!iter.ready || !iter.enableStatus || !iter.homeFinished || iter.errStatus)
    //         error_status = true;
    // }
    static int last_state = 65535;
    if (last_state != msg->periphstateinfo) {
        last_state = msg->periphstateinfo;
        MONITOR_INFO("[ModeMonitor] PeriphStatus:" << msg->periphstateinfo);
    }
    
    if (msg->periphstateinfo < 0) { 
        error_status = true;
    }

    if (error_status) {
        // MONITOR_WARN("[ModeMonitor] Peripheral status error!");
        periph_status_.second = ERROR;
    } else {
        periph_status_.second = NO_ERROR;
    }
    periph_status_.first = Monitor::GetTimes();
}

void ModeMonitor::EnableChissisController(const rclcpp::Time time)
{
    std::lock_guard<std::mutex> lock(mutex_);
    bool chassis_change_enable = false;
    bool enable = false;
    bool disable = false;

    int estop_current_state = monitor::Monitor::GetVehicleSaftyState();
    
    auto request = std::make_shared<agv_srvs::srv::ControllerEnable::Request>();
    request->controller_names.push_back("chassis_controller");

    if (estop_current_state == EMERGENCYSTOP && 
        cur_mode_ == Mode_Manul && 
        estop_last_state_ != EMERGENCYSTOP && 
        chassis_enable_state_) {

        request->enable = false;
        chassis_change_enable = true;
        disable = true;

    } else if (estop_current_state != EMERGENCYSTOP &&
        cur_mode_ == Mode_Manul) {
        estop_last_state_ = estop_current_state;
        
    } else if (estop_current_state != EMERGENCYSTOP && 
        cur_mode_ == Mode_Master_Auto && 
        !chassis_enable_state_) {

        request->enable = true;
        chassis_change_enable = true;
        enable = true;
    }

    MONITOR_INFO("[ModeMonitor] estop current state:" << estop_current_state << 
        ", estop last state:" << estop_last_state_ << 
        ", cur_mode:" << cur_mode_ << 
        ", chassis_enable_state:" << chassis_enable_state_ << 
        ", chassis_change_enable:" << chassis_change_enable);
   
    if (chassis_change_enable) {
        if (time - last_request_ < rclcpp::Duration(1, 0)) { // Already request and return
            tiggerEvent(agv_msgs::msg::AGVEvent::NONE, agv_msgs::msg::AGVEvent::WARNING, 
                        true, agv_msgs::msg::AGVEvent::CHISSIS_ENABLE_ERROR,
                        0, 0, "chassis controller serivce request fast trigger interval", time);
            MONITOR_INFO("[ModeMonitor] Already request delta :" << time.seconds() - last_request_.seconds());
            return;
        }

        if (!chassis_controller_client_->service_is_ready()) {
            MONITOR_ERROR("[ModeMonitor] chassis controller service is not ready!");
            return;
        } 

        MONITOR_INFO("[mode monitor] call chassis serivce");

        try {
            using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::ControllerEnable>::SharedFuture;
            auto response_received_callback = [this, &enable, &disable](ServiceResponseFuture future) {
                if (enable) chassis_enable_state_ = true;
                else if (disable) chassis_enable_state_ = false;
                MONITOR_INFO("[ModeMonitor] chassis controller service success. state:" << chassis_enable_state_);
            };

            auto result = chassis_controller_client_->async_send_request(request, response_received_callback);
        } catch (const std::exception &e) {
            tiggerEvent(agv_msgs::msg::AGVEvent::NONE, agv_msgs::msg::AGVEvent::WARNING, true, 
                        agv_msgs::msg::AGVEvent::CHISSIS_ENABLE_ERROR,
                        0, 0, e.what(), time);
            MONITOR_ERROR("[ModeMonitor] call chassis controller service error");
        }
        
        manual_automatic_button_trigger_ = false;
        last_request_ = time;
    }
}

void ModeMonitor::PoseCallBack(const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr &msg)
{
    if (!location_is_running_) {
        location_is_running_ = true;
    }

    if (msg->score >= 20.0f && !has_located_) {
        has_located_ = true;
        location_time_ = Monitor::GetTimes();
        MONITOR_INFO("[ModeMonitor] receive loaction node msg score:"<< msg->score <<
             " time:" << location_time_.seconds());
    }
}

void ModeMonitor::on_button_pressed(const std::string& name, const rclcpp::Time time, const bool state)
{
    rclcpp::Time now = Monitor::GetTimes();
    // MONITOR_INFO("name:" << name << ", state:" << state);
    if (name == keyname[0]) {
        if (mode_key_input_[0].state != state) {
            mode_key_input_[0].state = state;
            if (state) {
                pre_mode_ = Mode_Master_Auto;
            } else {
                pre_mode_ = Mode_Manul;
            }
            MONITOR_INFO("[ModeMonitor] manul input button set mode:" << pre_mode_);
        }
        mode_key_input_[0].last_press_time = time;
    } else if (name == keyname[1]) {
        mode_key_input_[1].state = state;
        auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
            std::chrono::duration<double>(mode_key_input_[1].long_press_threshold));

        if (state && !mode_key_input_[1].triggered) {
            mode_key_input_[1].triggered = true;
            mode_key_input_[1].last_press_time = time;
            MONITOR_INFO("[ModeMonitor] mode button triggered");
        } else if (state && mode_key_input_[1].triggered && !manual_automatic_button_trigger_ &&
            (now - mode_key_input_[1].last_press_time) > rclcpp::Duration(ns)) {
            if (cur_mode_ == Mode_Master_Auto) {
                pre_mode_ = Mode_Manul;
            } else if (cur_mode_ == Mode_Manul) {
                pre_mode_ = Mode_Master_Auto;
            }
            manual_automatic_button_trigger_ = true;
            mode_key_input_[1].last_press_time = time;
            MONITOR_INFO("[ModeMonitor] mode input button set mode:" << pre_mode_);
        } else if (!state && mode_key_input_[1].triggered) {
            mode_key_input_[1].triggered = false;
            manual_automatic_button_trigger_ = false;
            MONITOR_INFO("[ModeMonitor] reset mode button triggered flag");
        }
    }
}

void ModeMonitor::ReadInput()
{
    if (!initialize_agv_mode_)
        return;

    std::vector<agv_msgs::msg::PairTypeInt> inputs = Monitor::inputs.second.array;

    for (auto &input : inputs) {
        for (int i=0;i<MODEKEYCOUNT;i++) {
            if (input.key == mode_key_input_[i].name) {
                bool input_level = (input.value != 0);
                if (mode_key_input_[i].handler_ != nullptr) {
                    mode_key_input_[i].handler_(mode_key_input_[i].name, Monitor::inputs.first, input_level);
                }
            }
        }
    }
}

void ModeMonitor::AgvModePublish()
{
    static uint32_t seq = 0;
    if (realtime_agv_mode_pub_->trylock()) {
        auto& mode = realtime_agv_mode_pub_->msg_;
        mode.mode = cur_mode_;
        mode.header.stamp = Monitor::GetTimes();
        realtime_agv_mode_pub_->unlockAndPublish();
    }

    agv_msgs::msg::NavType navtype;
    navtype.type = getNavType();

    // read param from ros param
    int size = 0;
    std::string cur_floor = "";
    std::tie(cur_floor, size) = getFloorParam();
    floor_.resize(size);
    navtype.currentfloor = cur_floor;
    navtype.floors = floor_.floors;
    nav_type_pub_->publish(navtype);
}

void ModeMonitor::CheckEventState()
{
    bool error_status = false;
    AgvEventMap *eventmap = EventManager::Instance()->GetEventMap();
    ASSERT(eventmap != nullptr);

    for (auto it = eventmap->begin(); it != eventmap->end(); it++) {
        AgvEventVector events = it->second;
        // Using a range-based for loop
        for (const auto &event : events) {
            switch (event.second.action) 
            {
                case agv_msgs::msg::AGVEvent::ESTOP:
                case agv_msgs::msg::AGVEvent::STOP:
                case agv_msgs::msg::AGVEvent::DELAYSTOP:
                case agv_msgs::msg::AGVEvent::SLOWDONE:
                    error_status = true;
                    break;
                default:
                    break;
            }
        }
    }

    if (error_status) {
        MONITOR_WARN("[mode monitor] Event status error!");
        event_status_.second = ERROR;
    } else {
        event_status_.second = NO_ERROR;
    }
}

void ModeMonitor::SwitchMode(const rclcpp::Time time)
{
    rclcpp::Duration duration(2, 0); // 2s timeout

    // delay 15s wait location node and naviagite node to start.
    if (!initialize_agv_mode_) {
        if ((location_is_running_ && has_located_)) {
            if (time - location_time_ >= rclcpp::Duration(3, 0)) {
                if (init_mode_ == Mode_Local_Auto || init_mode_ == Mode_Master_Auto) {
                    if (periph_status_.second == NO_ERROR) {
                        initialize_agv_mode_ = true;
                        pre_mode_ = init_mode_;
                    } else {
                        MONITOR_INFO("[ModeMonitor] wait for periph status!");
                    }  
                } else {
                    initialize_agv_mode_ = true;
                    pre_mode_ = init_mode_;
                }

                if (initialize_agv_mode_) {
                    MONITOR_INFO("[ModeMonitor] AGV mode init ok, cur mode:" << cur_mode_ << " pre mode:" <<
                        pre_mode_ << " location node run:" << location_is_running_ << " location delay:" << 
                        (time.seconds() - location_time_.seconds()) << " start delay:" << 
                        (time.seconds() - start_time_.seconds()));
                }
            }
        } else if (time - start_time_ >= start_delay_time_) { // time out, exit init mode
            initialize_agv_mode_ = true;
            tiggerEvent(agv_msgs::msg::AGVEvent::NONE, agv_msgs::msg::AGVEvent::WARNING, true, 
                        agv_msgs::msg::AGVEvent::SWITCH_MODE_EVENT,
                        init_mode_, cur_mode_, "AGV mode init failed", time);

            MONITOR_WARN("[ModeMonitor] AGV mode init failed, mode:" << cur_mode_ << 
                    " location node run:" << location_is_running_ << 
                    " location delay:" << (time.seconds() - location_time_.seconds()) <<
                    " start delay:" << (time.seconds() - start_time_.seconds()));
        }

        cur_nav_type_ = getNavType();
        if (cur_nav_type_ != init_nav_type_) {
            setNavType(init_nav_type_);
            cur_nav_type_ = init_nav_type_;
            last_nav_type_ = cur_nav_type_;
        }
    }

    ReadInput();

    if (pre_mode_ != Mode_Idle) {
        bool allow_switching = false;
        // CheckEventState();
        switch (pre_mode_) {
            case Mode_Manul:
                // allow_switching = true;
                // break;
            case Mode_Semi_Auto:
                // if (event_status_.second == NO_ERROR)
                allow_switching = true;
                // break;
            case Mode_Local_Auto:
            case Mode_Master_Auto:
                if (shielded_peripherals_status_)  {
                    allow_switching = true;
                } else {
                    if (/*event_status_.second == NO_ERROR &&*/ periph_status_.second == NO_ERROR) {
                        if (time - periph_status_.first < duration) {
                            allow_switching = true;
                        } else {
                            MONITOR_ERROR("[ModeMonitor] periph status time out:" << periph_status_.first.seconds());
                            tiggerEvent(agv_msgs::msg::AGVEvent::NONE, agv_msgs::msg::AGVEvent::WARNING, true, 
                                        agv_msgs::msg::AGVEvent::SWITCH_MODE_EVENT,
                                        pre_mode_, cur_mode_, 
                                        "Periph status error: switch to AGV mode failed. Please check.", time);
                        }
                    } else {
                        MONITOR_WARN("[ModeMonitor] periph status Erro!");
                    }
                }
                break;
            default:
                break;
        }
        
        if (allow_switching) {
            cur_mode_ = pre_mode_;
            MONITOR_INFO("[ModeMonitor] Allow switch mode -- current mode: " << cur_mode_ << ", new mode :" << pre_mode_);
            pre_mode_ = Mode_Idle;

            estop_last_state_ = monitor::Monitor::GetVehicleSaftyState();
        } else {
            MONITOR_WARN("[ModeMonitor] Not allow switch mode -- current mode: " << cur_mode_ << ", new mode :" << pre_mode_);
            pre_mode_ = Mode_Idle;
        }

        monitor::ModeMonitor::SetVehicleMode((int)cur_mode_);
    }

    // vehicle chissis controller enable
    if (monitor::Monitor::GetVehicleModel() == "amr" && initialize_agv_mode_ /*&& manual_automatic_button_trigger_*/) {
        std::thread ([&, time]() { EnableChissisController(time); }).detach();  
        if (manual_automatic_button_trigger_ && time - mode_key_input_[1].last_press_time > rclcpp::Duration(10, 0)) {
            manual_automatic_button_trigger_ = false; // clear flags
            MONITOR_INFO("[ModeMonitor] clear manual_automatic_button_trigger");
        }
    }
}

/*
 * trigger event.
 */

void ModeMonitor::tiggerEvent(const int action, const int type, const bool ackreq, const int ecode,
                              const int param1, const int param2, const std::string msg, const rclcpp::Time time)
{
    auto eventPtr = std::make_shared<agv_msgs::msg::AGVEvent>();
    eventPtr->header.stamp = time;
    eventPtr->header.frame_id = name_;
    eventPtr->type = type;
    eventPtr->action = action;
    eventPtr->ackreq = ackreq;
    eventPtr->eventtime = time.seconds();
    eventPtr->ecode = ecode;
    eventPtr->param1 = param1;
    eventPtr->param2 = param2;
    eventPtr->description = msg;
    EventManager::EscalateEvents("modeMonitor", eventPtr);
}

bool ModeMonitor::OperationModeCallback(const std::shared_ptr<agv_srvs::srv::SetOperationMode::Request> req, 
    std::shared_ptr<agv_srvs::srv::SetOperationMode::Response> res)
{
    MONITOR_INFO("OperationModeCallback sender:" << req->sender << ",mode:" << req->mode);
    // 请求切换到手动模式
    if (req->mode == MODE_REQUEST::MODE_MANUL) {
        if (req->sender == MODE_REQUEST::SENDER_SOURCE_JOY) {
            joystick_lock_ = true;
        }

        if (joystick_lock_ == false || req->sender == MODE_REQUEST::SENDER_SOURCE_JOY) {
            pre_mode_ = Mode_Manul;
            res->success = true;
            res->message = "Preoperation manual mode";
        } else {
            res->success = false;
            res->message = "Preoperation manual mode can not be set! check joystick button!";
        }
    } else if (req->mode == MODE_REQUEST::MODE_AUTO) { // 请求切换到自动模式
        // 手柄未锁住
        if (joystick_lock_ == false && req->sender != MODE_REQUEST::SENDER_SOURCE_JOY) {
            // 自动模式下的本地调试模式
            if (req->submode == MODE_REQUEST::SUB_MODE_LOCAL_AUTO) {
                pre_mode_ = Mode_Local_Auto;
                res->success = true;
                res->message = "Preoperation local mode";
            } else if (req->submode == MODE_REQUEST::SUB_MODE_MASTER_AUTO) { // 自动模式下的主机模式
                pre_mode_ = Mode_Master_Auto;
                res->success = true;
                res->message = "Preoperation master mode";
            } else { // 未知模式
                res->success = false;
                res->message = "Preoperation auto mode can not be set! check submode!";
            }
        } else if (req->sender == MODE_REQUEST::SENDER_SOURCE_JOY) { // 手柄切换
            joystick_lock_ = false;

            pre_mode_ = Mode_Master_Auto;
            res->success = true;
            res->message = "Preoperation master mode";
            // if (mode_date_.cur_auto_mode == Mode_Local_Auto) {
            //     pre_mode_ = Mode_Local_Auto;
            //     res->success = true;
            //     res->message = "agv local mode";
            // } else if (mode_date_.cur_auto_mode == Mode_Master_Auto) {
            //     pre_mode_ = Mode_Master_Auto;
            //     res->success = true;
            //     res->message = "agv master mode";
            // } else {
            //     MONITOR_INFO("cur_auto_mode=" << mode_date_.cur_auto_mode);
            //     res->message = "agv submode error";
            //     res->success = false;
            // }
        } else {
            res->success = false;
            res->message = "Preoperation auto mode can not be set! check joystick button!";
        }
    } else if (req->mode == MODE_REQUEST::MODE_HALF_AUTO) {
        if (req->sender == MODE_REQUEST::SENDER_SOURCE_JOY) {
            joystick_lock_ = true;
        }

        if (joystick_lock_ == false || req->sender == MODE_REQUEST::SENDER_SOURCE_JOY) {
            pre_mode_ = Mode_Semi_Auto;
            res->success = true;
            res->message = "Preoperation Semi auto mode";
        } else {
            res->success = false;
            res->message = "Preoperation Semi auto mode can not be set! check joystick button!";
        }
    } else {
        res->success = false;
        res->message = "mode error! check again!";
    }

    if (!initialize_agv_mode_)
        initialize_agv_mode_ = true;

    MONITOR_INFO("[mode monitor] change curMode:" << pre_mode_ << ", message:" << res->message);
    return true;
}

}
